/*
 *Low level delay functions.
 *This code is modified from Linux source,
 *please atribute your credits to Linux developpers, if you want.
 */
#include <cnix/kernel.h>

extern unsigned long loops_per_jiffy;

/* simple loop based delay: */
static void __delay(unsigned long loops)
{
  asm volatile(
    " test %0,%0  \n"
    " jz 3f   \n"
    " jmp 1f    \n"

    ".align 16    \n"
    "1: jmp 2f    \n"

    ".align 16    \n"
    "2: dec %0    \n"
    " jnz 2b    \n"
    "3: dec %0    \n"

    : /* we don't need output */
    :"a" (loops)
  );
}

/*
 * This is the number of bits of precision for the loops_per_jiffy.  Each
 * bit takes on average 1.5/HZ seconds.  This (like the original) is a little
 * better than 1%
 * For the boot cpu we can skip the delay calibration and assign it a value
 * calculated based on the timer frequency.
 * For the rest of the CPUs we cannot assume that the timer frequency is same as
 * the cpu frequency, hence do the calibration for those.
 */
#define LPS_PREC 8

void calibrate_delay(void)
{
  unsigned long ticks, loopbit;
  int lps_precision = LPS_PREC;
	
  loops_per_jiffy = (1<<12);

  printk("Calibrating delay loop... ");
  while ((loops_per_jiffy <<= 1) != 0) {
    /* wait for "start of" clock tick */
    ticks = nowticks;
    while (ticks == nowticks)
      /* nothing */;
    /* Go .. */
    ticks = nowticks;
    __delay(loops_per_jiffy);
    ticks = nowticks - ticks;
    if (ticks)
      break;
  }

  /*
   * Do a binary approximation to get loops_per_jiffy set to
   * equal one clock (up to lps_precision bits)
   */
  loops_per_jiffy >>= 1;
  loopbit = loops_per_jiffy;
  while (lps_precision-- && (loopbit >>= 1)) {
    loops_per_jiffy |= loopbit;
    ticks = nowticks;
    while (ticks == nowticks)
      /* nothing */;
    ticks = nowticks;
    __delay(loops_per_jiffy);
    if (nowticks!= ticks) /* longer than 1 tick */
      loops_per_jiffy &= ~loopbit;
  }
  printk("%u.%u BogoMIPS (lpj=%u)\n",
    loops_per_jiffy/(500000/HZ),
    (loops_per_jiffy/(5000/HZ)) % 100, loops_per_jiffy);
}

inline void __const_udelay(unsigned long xloops)
{
  int d0;

  xloops *= 4;
  asm("mull %%edx"
    :"=d" (xloops), "=&a" (d0)
    :"1" (xloops), "0"
    (loops_per_jiffy * (HZ/4)));

  __delay(++xloops);
}

void udelay(unsigned long usecs)
{
  __const_udelay(usecs * 0x000010c7); /* 2**32 / 1000000 (rounded up) */
}

void msleep(unsigned long ms)
{
	udelay(ms*1000);
}

void ndelay(unsigned long nsecs)
{
  __const_udelay(nsecs * 0x00005); /* 2**32 / 1000000000 (rounded up) */
}
